/**
 *
 * @author Pawel Jankowski
 */

public class Controller {

    private NXTFuzzyLogic nfl = new NXTFuzzyLogic();

    public Controller() {

        nfl = new NXTFuzzyLogic();

        nfl.defineInputLinguisticVariable("vel", -10000, 10000, 0);
        nfl.defineInputLinguisticVariable("angle", -2000, 2000, 0);

        nfl.defineTermZType("backLarge", "vel", -75, -50);
        nfl.defineTermTriangular("backMedium", "vel", -75, -50, -25);
        nfl.defineTermTriangular("backSmall", "vel", -50, -25, 0);
        nfl.defineTermTriangular("zero", "vel", -25, 0, 25);
        nfl.defineTermTriangular("fowSmall", "vel", 0, 25, 50);
        nfl.defineTermTriangular("fowMedium", "vel", 25, 50, 75);
        nfl.defineTermSType("fowLarge", "vel", 50, 75);

        nfl.defineTermZType("negLarge", "angle", -9, -6);
        nfl.defineTermTriangular("negMedium", "angle", -9, -6, -3);
        nfl.defineTermTriangular("negSmall", "angle", -6, -3, 0);
        nfl.defineTermTriangular("zeroAng", "angle", -3, 0, 3);
        nfl.defineTermTriangular("posSmall", "angle", 0, 3, 6);
        nfl.defineTermTriangular("posMedium","angle", 3, 6, 9);
        nfl.defineTermSType("posLarge", "angle", 6, 9);

        nfl.defineOutputLinguisticVariable("speed", -1000, 1000, 0, NXTFuzzyLogic.COG);

        nfl.defineTermZType("backVeryFast", "speed", -900, -450);
        nfl.defineTermTriangular("backFast", "speed", -550, -300, -200);
        nfl.defineTermTriangular("backMedium", "speed", -250, -200, -100);
        nfl.defineTermTriangular("backSlow", "speed", -150, -100, -50);
        nfl.defineTermTriangular("backVerySlow", "speed", -80, -40, 0);
        nfl.defineTermTriangular("stop", "speed", -5, 0, 5);
        nfl.defineTermTriangular("fowVerySlow", "speed", 0, 40, 80);
        nfl.defineTermTriangular("fowSlow", "speed", 50, 100, 150);
        nfl.defineTermTriangular("fowMedium", "speed", 100, 200, 250);
        nfl.defineTermTriangular("fowFast", "speed", 200, 300, 550);
        nfl.defineTermSType("fowVeryFast", "speed", 450, 900);

        //kat - negLarge========================================================
        String[] r0 = {"negLarge", "backLarge"};
        nfl.defineRule(r0, "backVeryFast", NXTFuzzyLogic.MIN);
        String[] r1 = {"negLarge", "backMedium"};
        nfl.defineRule(r1, "backVeryFast", NXTFuzzyLogic.MIN);
        String[] r2 = {"negLarge", "backSmall"};
        nfl.defineRule(r2, "backFast", NXTFuzzyLogic.MIN);
        String[] r3 = {"negLarge", "zero"};
        nfl.defineRule(r3, "backMedium", NXTFuzzyLogic.MIN);
        String[] r4 = {"negLarge", "fowSmall"};
        nfl.defineRule(r4, "backSlow", NXTFuzzyLogic.MIN);
        String[] r5 = {"negLarge", "fowMedium"};
        nfl.defineRule(r5, "backVerySlow", NXTFuzzyLogic.MIN);
        String[] r6 = {"negLarge", "fowLarge"};
        nfl.defineRule(r6, "stop", NXTFuzzyLogic.MIN);

        //kat - negMedium ======================================================
        String[] r7 = {"negMedium", "backLarge"};
        nfl.defineRule(r7, "backVeryFast", NXTFuzzyLogic.MIN);
        String[] r8 = {"negMedium", "backMedium"};
        nfl.defineRule(r8, "backFast", NXTFuzzyLogic.MIN);
        String[] r9 = {"negMedium", "backSmall"};
        nfl.defineRule(r9, "backMedium", NXTFuzzyLogic.MIN);
        String[] r10 = {"negMedium", "zero"};
        nfl.defineRule(r10, "backSlow", NXTFuzzyLogic.MIN);
        String[] r11 = {"negMedium", "fowSmall"};
        nfl.defineRule(r11, "backVerySlow", NXTFuzzyLogic.MIN);
        String[] r12 = {"negMedium", "fowMedium"};
        nfl.defineRule(r12, "stop", NXTFuzzyLogic.MIN);
        String[] r13 = {"negMedium", "fowLarge"};
        nfl.defineRule(r13, "fowVerySlow", NXTFuzzyLogic.MIN);

        //kat - negSmall========================================================
        String[] r14 = {"negSmall", "backLarge"};
        nfl.defineRule(r14, "backFast", NXTFuzzyLogic.MIN);
        String[] r15 = {"negSmall", "backMedium"};
        nfl.defineRule(r15, "backMedium", NXTFuzzyLogic.MIN);
        String[] r16 = {"negSmall", "backSmall"};
        nfl.defineRule(r16, "backSlow", NXTFuzzyLogic.MIN);
        String[] r17 = {"negSmall", "zero"};
        nfl.defineRule(r17, "backVerySlow", NXTFuzzyLogic.MIN);
        String[] r18 = {"negSmall", "fowSmall"};
        nfl.defineRule(r18, "stop", NXTFuzzyLogic.MIN);
        String[] r19 = {"negSmall", "fowMedium"};
        nfl.defineRule(r19, "fowVerySlow", NXTFuzzyLogic.MIN);
        String[] r20 = {"negSmall", "fowLarge"};
        nfl.defineRule(r20, "fowSlow", NXTFuzzyLogic.MIN);


        //kat - zeroAng=========================================================
        String[] r21 = {"zeroAng", "backLarge"};
        nfl.defineRule(r21, "backMedium", NXTFuzzyLogic.MIN);
        String[] r22 = {"zeroAng", "backMedium"};
        nfl.defineRule(r22, "backSlow", NXTFuzzyLogic.MIN);
        String[] r23 = {"zeroAng", "backSmall"};
        nfl.defineRule(r23, "backVerySlow", NXTFuzzyLogic.MIN);
        String[] r24 = {"zeroAng", "zero"};
        nfl.defineRule(r24, "stop", NXTFuzzyLogic.MIN);
        String[] r25 = {"zeroAng", "fowSmall"};
        nfl.defineRule(r25, "fowVerySlow", NXTFuzzyLogic.MIN);
        String[] r26 = {"zeroAng", "fowMedium"};
        nfl.defineRule(r26, "fowSlow", NXTFuzzyLogic.MIN);
        String[] r27 = {"zeroAng", "fowLarge"};
        nfl.defineRule(r27, "fowMedium", NXTFuzzyLogic.MIN);


        //kat - posSmall========================================================
        String[] r28 = {"posSmall", "backLarge"};
        nfl.defineRule(r28, "backSlow", NXTFuzzyLogic.MIN);
        String[] r29 = {"posSmall", "backMedium"};
        nfl.defineRule(r29, "backVerySlow", NXTFuzzyLogic.MIN);
        String[] r30 = {"posSmall", "backSmall"};
        nfl.defineRule(r30, "stop", NXTFuzzyLogic.MIN);
        String[] r31 = {"posSmall", "zero"};
        nfl.defineRule(r31, "fowVerySlow", NXTFuzzyLogic.MIN);
        String[] r32 = {"posSmall", "fowSmall"};
        nfl.defineRule(r32, "fowSlow", NXTFuzzyLogic.MIN);
        String[] r33 = {"posSmall", "fowMedium"};
        nfl.defineRule(r33, "fowMedium", NXTFuzzyLogic.MIN);
        String[] r34 = {"posSmall", "fowLarge"};
        nfl.defineRule(r34, "fowFast", NXTFuzzyLogic.MIN);

        //kat - posMedium=======================================================
        String[] r35 = {"posMedium", "backLarge"};
        nfl.defineRule(r35, "backVerySlow", NXTFuzzyLogic.MIN);
        String[] r36 = {"posMedium", "backMedium"};
        nfl.defineRule(r36, "stop", NXTFuzzyLogic.MIN);
        String[] r37 = {"posMedium", "backSmall"};
        nfl.defineRule(r37, "fowVerySlow", NXTFuzzyLogic.MIN);
        String[] r38 = {"posMedium", "zero"};
        nfl.defineRule(r38, "fowSlow", NXTFuzzyLogic.MIN);
        String[] r39 = {"posMedium", "fowSmall"};
        nfl.defineRule(r39, "fowMedium", NXTFuzzyLogic.MIN);
        String[] r40 = {"posMedium", "fowMedium"};
        nfl.defineRule(r40, "fowFast", NXTFuzzyLogic.MIN);
        String[] r41 = {"posMedium", "fowLarge"};
        nfl.defineRule(r41, "fowVeryFast", NXTFuzzyLogic.MIN);

        //kat - posLarge========================================================
        String[] r42 = {"posLarge", "backLarge"};
        nfl.defineRule(r42, "stop", NXTFuzzyLogic.MIN);
        String[] r43 = {"posLarge", "backMedium"};
        nfl.defineRule(r43, "fowVerySlow", NXTFuzzyLogic.MIN);
        String[] r44 = {"posLarge", "backSmall"};
        nfl.defineRule(r44, "fowSlow", NXTFuzzyLogic.MIN);
        String[] r45 = {"posLarge", "zero"};
        nfl.defineRule(r45, "fowMedium", NXTFuzzyLogic.MIN);
        String[] r46 = {"posLarge", "fowSmall"};
        nfl.defineRule(r46, "fowFast", NXTFuzzyLogic.MIN);
        String[] r47 = {"posLarge", "fowMedium"};
        nfl.defineRule(r47, "fowVeryFast", NXTFuzzyLogic.MIN);
        String[] r48 = {"posLarge", "fowLarge"};
        nfl.defineRule(r48, "fowVeryFast", NXTFuzzyLogic.MIN);

        nfl.init();
    }

    public void setInput(int angularVelocity, int angle){
        nfl.setInputValue("vel", angularVelocity);
        nfl.setInputValue("angle", angle);
    }

    public int getOutput(){
        nfl.evaluate();
        return (int) nfl.getOutputValue("speed");
    }
}
